#include "main.h"

using namespace std;

//#define IP "127.0.0.1"
#define IP "192.168.10.100"
// #define IP2 "192.168.10.101"

#define DEV "DEV:"<<DevID << " "


void save( char *fileName , char *data , int len)
{
    FILE *pFile;
    pFile = fopen( fileName , "w+");
    if(pFile == NULL)
    {
        return ;
    }
    fwrite(data, 1, len, pFile);
    fclose(pFile);
}

int parseIntrinsicParameters(const char* pInfo) {
    picojson::value mIntrinsicsJson;
    picojson::object mObj;
    picojson::object mObjRGB;
    picojson::object mObjExt;
    int rgb_flag = 0;

    std::string err = picojson::parse(mIntrinsicsJson, std::string(pInfo));
    if (!err.empty()) {
        return -1;
    }

    mObj = mIntrinsicsJson.get<picojson::object>();
    
    // 解析TOF参数
    mParam.Width_tof = static_cast<int>(mObj["width"].get<double>());
    mParam.Height_tof = static_cast<int>(mObj["height"].get<double>());
    mParam.fx_tof = mObj["f_x"].get<double>();
    mParam.fy_tof = mObj["f_y"].get<double>();
    mParam.cx_tof = mObj["c_x"].get<double>();
    mParam.cy_tof = mObj["c_y"].get<double>();
    mParam.k1_tof = mObj["k_1"].get<double>();
    mParam.k2_tof = mObj["k_2"].get<double>();
    mParam.k3_tof = mObj["k_3"].get<double>();
    mParam.p1_tof = mObj["p_1"].get<double>();
    mParam.p2_tof = mObj["p_2"].get<double>();
    
    // 解析RGB参数
    if(mObj["rgb"].is<picojson::object>())
    {  
        mObjRGB = mObj["rgb"].get<picojson::object>();
        // printf("--------------------read rgb\n");
        mParam.Width_rgb = static_cast<int>(mObjRGB["width"].get<double>());
        mParam.Height_rgb = static_cast<int>(mObjRGB["height"].get<double>());
        mParam.fx_rgb = mObjRGB["f_x"].get<double>();
        mParam.fy_rgb = mObjRGB["f_y"].get<double>();
        mParam.cx_rgb = mObjRGB["c_x"].get<double>();
        mParam.cy_rgb = mObjRGB["c_y"].get<double>();
        mParam.k1_rgb = mObjRGB["k_1"].get<double>();
        mParam.k2_rgb = mObjRGB["k_2"].get<double>();
        mParam.k3_rgb = mObjRGB["k_3"].get<double>();
        mParam.p1_rgb = mObjRGB["p_1"].get<double>();
        mParam.p2_rgb = mObjRGB["p_2"].get<double>();

        // 解析外参
        mObjExt = mObj["ext_params"].get<picojson::object>();
        
        // 解析旋转矩阵
        picojson::array rotation = mObjExt["Rotation"].get<picojson::array>();
        for (int i = 0; i < 3; ++i) {
            picojson::array row = rotation[i].get<picojson::array>();
            for (int j = 0; j < 3; ++j) {
                mParam.R[i][j] = row[j].get<double>();
                mParam.R_(i, j) = static_cast<float>(mParam.R[i][j]);  // 初始化 R_
            }
        }

        // 解析平移向量
        picojson::array translation = mObjExt["Translation"].get<picojson::array>();
        for (int i = 0; i < 3; ++i) {
            mParam.T[i] = translation[i].get<double>();
            mParam.T_(i) = static_cast<float>(mParam.T[i]);  // 初始化 T_
        }
        rgb_flag = 1;
    }
    // else if(mObj["RGB_width"].is<picojson::object>())
    else if(mObj.find("RGB_width") != mObj.end())
    {
        // printf("-------------------------read rgb failed\n");

        mParam.Width_rgb = static_cast<int>(mObj["RGB_width"].get<double>());
        // printf("-------------------------read RGB failed\n");
        mParam.Height_rgb = static_cast<int>(mObj["RGB_height"].get<double>());
        mParam.fx_rgb = mObj["RGB_f_x"].get<double>();
        mParam.fy_rgb = mObj["RGB_f_y"].get<double>();
        // printf("-------------------------read rgb failed\n");
        mParam.cx_rgb = mObj["RGB_c_x"].get<double>();
        mParam.cy_rgb = mObj["RGB_c_y"].get<double>();
        // printf("-------------------------read rgb failed\n");
        mParam.k1_rgb = mObj["RGB_k_1"].get<double>();
        mParam.k2_rgb = mObj["RGB_k_2"].get<double>();
        mParam.k3_rgb = mObj["RGB_k_3"].get<double>();
        // printf("-------------------------read rgb failed\n");
        mParam.p1_rgb = mObj["RGB_p_1"].get<double>();
        mParam.p2_rgb = mObj["RGB_p_2"].get<double>();
        // printf("-------------------------read rgb failed\n");

        // 解析外参        
        // 解析旋转矩阵
        picojson::array rotation = mObj["Rotation"].get<picojson::array>();
        for (int i = 0; i < 3; ++i) {
            picojson::array row = rotation[i].get<picojson::array>();
            for (int j = 0; j < 3; ++j) {
                mParam.R[i][j] = row[j].get<double>();
                mParam.R_(i, j) = static_cast<float>(mParam.R[i][j]);  // 初始化 R_
            }
        }

        // 解析平移向量
        picojson::array translation = mObj["Translation"].get<picojson::array>();
        for (int i = 0; i < 3; ++i) {
            mParam.T[i] = translation[i].get<double>();
            mParam.T_(i) = static_cast<float>(mParam.T[i]);  // 初始化 T_
        }
        rgb_flag = 1;
    }
    else
    {
        printf("-------------------------read rgb parameter failed\n");
    }

    // 打印所有参数
    std::cout << "\n=== Camera Parameters ===" << std::endl;

    // 打印TOF相机内参
    std::cout << "\nTOF Intrinsics:" << std::endl;
    std::cout << "  Resolution: " << mParam.Width_tof << "x" << mParam.Height_tof << std::endl;
    std::cout << "  Focal Length: (" << std::fixed << std::setprecision(6) 
                << mParam.fx_tof << ", " << mParam.fy_tof << ")" << std::endl;
    std::cout << "  Principal Point: (" << mParam.cx_tof << ", " << mParam.cy_tof << ")" << std::endl;
    std::cout << "  Distortion Coefficients:" << std::endl;
    std::cout << "    k1: " << mParam.k1_tof << ", k2: " << mParam.k2_tof 
                << ", k3: " << mParam.k3_tof << std::endl;
    std::cout << "    p1: " << mParam.p1_tof << ", p2: " << mParam.p2_tof << std::endl;
    // 构建RGB内参矩阵 
    TOF_camera_matrix.at<float>(0, 0) = mParam.fx_tof;
    TOF_camera_matrix.at<float>(1, 1) = mParam.fy_tof;
    TOF_camera_matrix.at<float>(0, 2) = mParam.cx_tof;
    TOF_camera_matrix.at<float>(1, 2) = mParam.cy_tof;
    
    TOF_dist_coeffs.at<float>(0, 0) = mParam.k1_tof;
    TOF_dist_coeffs.at<float>(0, 1) = mParam.k2_tof;
    TOF_dist_coeffs.at<float>(0, 2) = mParam.p1_tof;
    TOF_dist_coeffs.at<float>(0, 3) = mParam.p2_tof;
    TOF_dist_coeffs.at<float>(0, 4) = mParam.k3_tof;
    // 预计算映射表
    /*
    if (isRunningOnOrin_) {
        computeUndistortMaps(TOF_camera_matrix, TOF_dist_coeffs, 
        cv::Size(mParam.Width_tof, mParam.Height_tof),
        TOF_Undistort_Map_1, TOF_Undistort_Map_2);
    } else {
        cv::initUndistortRectifyMap(
            TOF_camera_matrix, TOF_dist_coeffs, cv::Mat(),
            TOF_camera_matrix, cv::Size(mParam.Width_tof, mParam.Height_tof), CV_16SC2, TOF_Undistort_Map_1, TOF_Undistort_Map_2
        );
    }
    */
    if (rgb_flag)
    {
        // 计算rgbd内参
        mParam.Height_rgbd =resolution_height_;
        float P_rgb2rgbd;
        P_rgb2rgbd = (float)mParam.Height_rgbd / (float)mParam.Height_rgb;

        mParam.Width_rgbd = mParam.Width_rgb * P_rgb2rgbd;
        mParam.Width_rgbd_cut = mParam.Width_tof * (float)mParam.Height_rgbd / (float)mParam.Height_tof;
        mParam.fx_rgbd = mParam.fx_rgb * P_rgb2rgbd;
        mParam.fy_rgbd = mParam.fy_rgb * P_rgb2rgbd;
        mParam.cx_rgbd = mParam.cx_rgb * P_rgb2rgbd;
        mParam.cy_rgbd = mParam.cy_rgb * P_rgb2rgbd;

        // 构建RGB内参矩阵    
        RGB_camera_matrix.at<float>(0, 0) = mParam.fx_rgb;
        RGB_camera_matrix.at<float>(1, 1) = mParam.fy_rgb;
        RGB_camera_matrix.at<float>(0, 2) = mParam.cx_rgb;
        RGB_camera_matrix.at<float>(1, 2) = mParam.cy_rgb;
        
        // 构建RGB畸变系数
        RGB_dist_coeffs.at<float>(0, 0) = mParam.k1_rgb;
        RGB_dist_coeffs.at<float>(0, 1) = mParam.k2_rgb;
        RGB_dist_coeffs.at<float>(0, 2) = mParam.p1_rgb;
        RGB_dist_coeffs.at<float>(0, 3) = mParam.p2_rgb;
        RGB_dist_coeffs.at<float>(0, 4) = mParam.k3_rgb;
        /*
        // 预计算映射表
        if (isRunningOnOrin_) {
            computeUndistortMaps(RGB_camera_matrix, RGB_dist_coeffs, 
            cv::Size(mParam.Width_rgb, mParam.Height_rgb),
            RGB_Undistort_Map_1, RGB_Undistort_Map_2);
        } else {
            cv::initUndistortRectifyMap(
                RGB_camera_matrix, RGB_dist_coeffs, cv::Mat(),
                RGB_camera_matrix, cv::Size(mParam.Width_rgb, mParam.Height_rgb), CV_16SC2, RGB_Undistort_Map_1, RGB_Undistort_Map_2
            );            
        }
        */
        
        // 打印RGB相机内参
        std::cout << "\nRGB Intrinsics:" << std::endl;
        std::cout << "  Resolution: " << mParam.Width_rgb << "x" << mParam.Height_rgb << std::endl;
        std::cout << "  Focal Length: (" << mParam.fx_rgb << ", " << mParam.fy_rgb << ")" << std::endl;
        std::cout << "  Principal Point: (" << mParam.cx_rgb << ", " << mParam.cy_rgb << ")" << std::endl;
        std::cout << "  Distortion Coefficients:" << std::endl;
        std::cout << "    k1: " << mParam.k1_rgb << ", k2: " << mParam.k2_rgb 
                    << ", k3: " << mParam.k3_rgb << std::endl;
        std::cout << "    p1: " << mParam.p1_rgb << ", p2: " << mParam.p2_rgb << std::endl;

        // 打印RGBD内参
        std::cout << "\nRGBD Intrinsics:" << std::endl;
        std::cout << "  Resolution: " << mParam.Width_rgbd << "x" << mParam.Height_rgbd << std::endl;
        std::cout << "  Focal Length: (" << mParam.fx_rgbd << ", " << mParam.fy_rgbd << ")" << std::endl;
        std::cout << "  Principal Point: (" << mParam.cx_rgbd << ", " << mParam.cy_rgbd << ")" << std::endl;

        // 打印外参
        std::cout << "\nExtrinsic Parameters:" << std::endl;
        std::cout << "  Rotation Matrix:" << std::endl;
        for(int i = 0; i < 3; ++i) {
            std::cout << "    [ ";
            for(int j = 0; j < 3; ++j) {
                std::cout << std::fixed << std::setw(10) 
                            << std::setprecision(6) << mParam.R[i][j];
                if(j < 2) std::cout << ", ";
            }
            std::cout << " ]" << std::endl;
        }

        std::cout << "  Translation Vector:" << std::endl;
        std::cout << "    [ ";
        for(int i = 0; i < 3; ++i) {
            std::cout << std::fixed << std::setw(10) 
                        << std::setprecision(6) << mParam.T[i];
            if(i < 2) std::cout << ", ";
        }
        std::cout << " ]\n" << std::endl;
    }
    return 1;
}

void alignDepthToRGB_Resize( 
    cv::Mat &depth_aligned, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr &rgb_cloud, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr &tof_cloud) {
    // 创建RGB分辨率的空深度图
    depth_aligned = cv::Mat(mParam.Height_rgbd, mParam.Width_rgbd, CV_32F, cv::Scalar(0));

    for (const auto &point_depth : tof_cloud->points) 
    {
        if (!std::isnan(point_depth.x)) 
        {
            Eigen::Vector3f point_depth_eigen(point_depth.x, point_depth.y, point_depth.z);
            Eigen::Vector3f point_rgb = mParam.R_ * point_depth_eigen + mParam.T_;

            if (point_rgb[2] > 0) 
            {
                float rgb_x = (point_rgb[0] / point_rgb[2] * mParam.fx_rgbd + mParam.cx_rgbd);
                float rgb_y = (point_rgb[1] / point_rgb[2] * mParam.fy_rgbd + mParam.cy_rgbd);

                if (rgb_x >= 0 && rgb_x < mParam.Width_rgbd && rgb_y >= 0 && rgb_y < mParam.Height_rgbd) 
                {
                    int rgb_u = static_cast<int>(rgb_x + 0.5f);
                    int rgb_v = static_cast<int>(rgb_y + 0.5f);

                    if (rgb_u >= 0 && rgb_u < depth_aligned.cols && rgb_v >= 0 && rgb_v < depth_aligned.rows) {
                        depth_aligned.at<float>(rgb_v, rgb_u) = point_depth.z;
                    }
                }
            }
        }
    }

    rgb_cloud->width = rgb_cloud->points.size();
    rgb_cloud->height = 1;
    rgb_cloud->is_dense = false;
}

void test(void *ip )
{
    int DevID = 0;
    struct timeval startTime , endTime;
    STRC_IMG_ALL *frame;
    int mNumFrame = 0;
    char *pInfo = NULL;
    char *mIP = (char *)ip;
    char mFileName[1024];
    init_flag_ = false;
    resolution_height_ = 480;

    cout << "SDK VER:" << api_get_sdk_ver() << endl;

    //connect
    DevID = api_connect((char *)mIP );
    cout << "DevID:" << DevID<< endl;
    if(DevID < 0)
    {
        cout << "api_connect failed." << endl;
        return ;
    }
    // usleep(10000);
    sleep(5);
    pInfo = api_get_dev_ver(DevID);
    if(pInfo == NULL)
    {
        cout << DEV << "get DEV version failed." << endl;
    }else{
        cout << DEV << "DEV VER:" << pInfo << endl; 
    }
    
    pInfo = api_get_intrinsic_parameters(DevID);
    if(pInfo == NULL)
    {
        cout << DEV << "get device intrinsic parameters info failed." << endl;
    }else{
        cout << DEV << "Device intrinsic parameters:" << endl; 
        init_flag_ = parseIntrinsicParameters(pInfo);
    }

    {//曝光参数
        int mCurExp , mMinExp , mMaxExp;
        mCurExp = api_get_exposure(DevID , &mMinExp , &mMaxExp);
        if(mCurExp < 0)
        {
            cout << DEV << "Get exposure parameter failed. error code:" << mCurExp << endl;
        }else{
            cout << DEV << "CurExp:"<<mCurExp << "  MinExp:" << mMinExp << "  MaxExp:"<<mMaxExp << endl;
        }
    }

    //tof SN
    while ((pInfo = api_get_sn_tof(DevID)) == NULL);
    cout << DEV << "TOF SN:" << pInfo << endl;


    //设置曝光
    api_set_exposure(DevID , 20);
    {//曝光参数
        int mCurExp , mMinExp , mMaxExp;
        mCurExp = api_get_exposure(DevID , &mMinExp , &mMaxExp);
        if(mCurExp < 0)
        {
            cout << DEV << "Get exposure parameter failed. error code:" << mCurExp << endl;
        }else{
            cout << DEV << "CurExp:"<<mCurExp << "  MinExp:" << mMinExp << "  MaxExp:"<<mMaxExp << endl;
        }
    }
    // 获取图像
    while((frame = api_get_img(DevID) ) == NULL);
    sleep(0.01);
    while((frame = api_get_img(DevID) ) == NULL);
    // 解析DEPTH图像
    cv::Mat depth(mParam.Height_tof, mParam.Width_tof, CV_32F, cv::Scalar(0));
    for (int i = 0; i < mParam.Height_tof; ++i) {
        for (int j = 0; j < mParam.Width_tof; ++j) {
            float &pixel = depth.at<float>(i, j);
            pixel = static_cast<float>(frame->img_depth.data[i * mParam.Width_tof + j]);
        }
    }
    cv::Mat depth_uint16;
    depth.convertTo(depth_uint16, CV_16U); // 将float转换为uint16
    cv::imwrite("depth_image.png", depth_uint16);
    // 获取IR图像
    cv::Mat ir(mParam.Height_tof, mParam.Width_tof, CV_32F, cv::Scalar(0));
    for (int i = 0; i < mParam.Height_tof; ++i) {
        for (int j = 0; j < mParam.Width_tof; ++j) {
            float &pixel = ir.at<float>(i, j);
            pixel = static_cast<float>(frame->img_amplitude.data[i * mParam.Width_tof + j]);
        }
    }
    if (init_flag_)
    {
        // IR图像去畸变
        cv::Mat undistorted_ir;
        cv::undistort(ir, undistorted_ir, TOF_camera_matrix, TOF_dist_coeffs);
        cv::Mat ir_undistorted_uint16;
        undistorted_ir.convertTo(ir_undistorted_uint16, CV_16U); // 将float转换为uint16
        cv::imwrite("ir_image.png", ir_undistorted_uint16);
        
        // Depth2Cloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr tof_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for (int i = 0; i < mParam.Height_tof; ++i) {
            for (int j = 0; j < mParam.Width_tof; ++j) {
                float depth_value = depth.at<float>(i, j);
                if (depth_value > 0 && depth_value < 10000) {
                    pcl::PointXYZ point;
                    point.x = (j - mParam.cx_tof) * depth_value / mParam.fx_tof;
                    point.y = (i - mParam.cy_tof) * depth_value / mParam.fy_tof;
                    point.z = depth_value;
                    tof_cloud->points.push_back(point);
                } else {
                    // 无效点设置为NaN
                    pcl::PointXYZ point;
                    point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN();
                    tof_cloud->points.push_back(point);
                }
            }
        }
        tof_cloud->width = tof_cloud->points.size();
        tof_cloud->height = 1;
        tof_cloud->is_dense = false;
        // // 保存点云为pcd格式
        // sprintf(mFileName, "pcl-%d.pcd", DevID);
        // pcl::io::savePCDFile(mFileName, *tof_cloud);

        // 获取RGB图像
        if (frame->img_rgb.len != 0)
        {
            cv::Mat rgb(mParam.Width_rgb, mParam.Height_rgb, CV_8UC3, cv::Scalar(0, 0, 0));
            char *src_data = (char *) frame->img_rgb.data;
            std::vector <uchar> img_data(src_data, src_data + frame->img_rgb.len);
            rgb = cv::imdecode(img_data, cv::IMREAD_COLOR);
            // RGB图像去畸变
            cv::Mat undistorted_rgb;
            cv::undistort(rgb, undistorted_rgb, RGB_camera_matrix, RGB_dist_coeffs);
            cv::imwrite("rgb_image.png", undistorted_rgb);
            
            // RGBD对齐
            cv::Mat aligned_depth;
            pcl::PointCloud<pcl::PointXYZ>::Ptr rgb_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            cv::Mat rgbd_rgb;
            std::vector<unsigned char> rgbd_rgb_points;
            alignDepthToRGB_Resize(aligned_depth, rgb_cloud, tof_cloud);
            cv::resize(undistorted_rgb, rgbd_rgb, cv::Size(mParam.Width_rgbd, mParam.Height_rgbd));
            cv::imwrite("rgbd_rgb.png", rgbd_rgb);
            aligned_depth.convertTo(aligned_depth, CV_16U); // 将float转换为uint16
            cv::imwrite("rgbd_depth.png", aligned_depth);
        }
    }
    
    
}
    


int main()
{
    int ret = 0;
    api_init();
    thread th1(test, (void *)IP);
    // thread th2(test , (void *)IP2);
    th1.join();
    // th2.join();
    api_exit();
    return ret;
}
